from typing import Any
import numpy as np

class pose:
    def __init__(self, x=0,y=0,phi=0):
        self.x=x
        self.y=y
        self.phi=phi
    def update(self, x,y,phi):
        self.x=x
        self.y=y
        self.phi=phi
    def __str__(self):
        return f'x: {self.x:.6f}, y:{self.y:.6f}, phi:{self.phi}'
    def __repr__(self):
        return f'x: {self.x:.6f}, y:{self.y:.6f}, phi:{self.phi}'
    def get_norm(self):
        return (self.x**2+self.y**2)**0.5
    def rescale(self, scale):
        '''归一化后设置乘为scale的norm'''
        n=self.get_norm()
        if n==0:
            n=0.001
        self.x=self.x/n*scale
        self.y=self.y/n*scale
        
class poses:
    def __init__(self, pl=[pose()]*7):
        self.pl=pl
       
    def __setitem__(self, i, p):
        self.pl[i]=p
    
    def __getitem__(self, i):
        return self.pl[i]
    
    def get_arr(self):
        arr=np.zeros((len(self.pl),3))
        for i in range(len(self.pl)):
            arr[i]=np.array([self.pl[i].x, self.pl[i].y, self.pl[i].phi])
        return arr

class speed:
    def __init__(self, x=0,y=0):
        self.x=x
        self.y=y
    def update(self, x=0,y=0):
        self.x=x
        self.y=y
    def __str__(self):
        return f'x: {self.x:.4f}, y: {self.y:.4f}'
    def __repr__(self):
        return f'x: {self.x:.4f}, y: {self.y:.4f}'
    def get_norm(self):
        return (self.x**2+self.y**2)**0.5

class speeds:
    def __init__(self, vl=[speed()]*7):
        self.vl=vl

    def __setitem__(self, i, v):
        self.vl[i]=v
    
    def __getitem__(self, i):
        return self.vl[i]
        
    def get_arr(self):
        arr=np.zeros((len(self.vl),2))
        for i in range(len(self.vl)):
            arr[i]=np.array([self.vl[i].x, self.vl[i].y])
        return arr
    
    def set_arr(self, arr):
        for i in range(len(arr)):
            self.vl[i]=speed(arr[i][0], arr[i][1])
